#include "stm32f10x.h"
#include <stdlib.h>
#include "MyProject.h"
#include "mypid.h"


/******************************************************************************/
#define LED_blink    GPIOD->ODR^=(1<<15)
/******************************************************************************/
float target0;
float target;
float angle_x;
float angle_y;
float v1;
float v2;
PID_t pid_1;
PID_t pid_2;
/******************************************************************************/
void commander_run(void);
/******************************************************************************/
void GPIO_Config(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOC|RCC_APB2Periph_GPIOD|RCC_APB2Periph_AFIO, ENABLE);//使能GPIOA,GPIOB,GPIOC,AFIO;
	GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7;//PB6 PB7 I2C接口
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;      //通用推挽输出	
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;     //速度
	GPIO_Init(GPIOB, &GPIO_InitStructure);                //对选中管脚初始化
	GPIO_SetBits(GPIOB,GPIO_Pin_6|GPIO_Pin_7);          //高电平
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10|GPIO_Pin_11;//PB10 PB11 I2C接口
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;      //通用推挽输出	
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;     //速度
	GPIO_Init(GPIOB, &GPIO_InitStructure);                //对选中管脚初始化
	GPIO_SetBits(GPIOB,GPIO_Pin_10|GPIO_Pin_11);          //高电平
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15;         //PD15是LED
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;   //推挽输出	
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;  //速度
	GPIO_Init(GPIOD, &GPIO_InitStructure);             //对选中管脚初始化
	GPIO_SetBits(GPIOD,GPIO_Pin_15);                   //上电点亮LED
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;          //PA3是motor1的使能
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
	GPIO_Init(GPIOA, &GPIO_InitStructure);
	GPIO_ResetBits(GPIOA,GPIO_Pin_3);                  //低电平解除,Motor_init()中使能
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;          //PB1是motor2的使能
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
	GPIO_Init(GPIOB, &GPIO_InitStructure);
	GPIO_ResetBits(GPIOB,GPIO_Pin_1);                  //低电平解除,Motor_init()中使能
}
/******************************************************************************/
int main(void)
{
	GPIO_Config();
	uart_init(115200);
	uart4_init(115200);

	TIM2_PWM_Init();
	TIM3_PWM_Init();
	TIM4_1ms_Init();           //interrupt per 1ms
	
	delay_ms(1000);            //Wait for the system to stabilize
	MagneticSensor_Init0();
	MagneticSensor_Init();
	
	voltage_power_supply=12;   //V
	voltage_limit=12;           //V，最大值需小于12/1.732=6.9
	velocity_limit=20;         //rad/s angleOpenloop() and PID_angle() use it
	voltage_sensor_align=12;    //V  alignSensor() and driverAlign() use it
	torque_controller=Type_voltage;  //当前只有电压模式
	controller=Type_velocity;  //Type_torque;  //Type_angle; //
	target0=0;
	target=0;
	
	Motor_init0();
	Motor_init();
	Motor_initFOC0();
	Motor_initFOC();
	PID_init0();
	PID_init();
	printf("Motor ready.\r\n");
	
	pid_init(&pid_1, 0.00025f, 0.0003f, 0.0002f);            //pitch
    pid_setThreshold(&pid_1, 8.0f, 8.0f, 0.0f);
    pid_setSetpoint(&pid_1, 0.0f);
    pid_enable(&pid_1, 1);
	
	pid_init(&pid_2, 0.00015f, 0.00025f, 0.0002f);             //roll
    pid_setThreshold(&pid_2, 8.0f, 8.0f, 0.0f);
    pid_setSetpoint(&pid_2, 0.0f);
    pid_enable(&pid_2, 1);
	
	systick_CountMode();   //不能再调用delay_us()和delay_ms()函数
	
	uint8_t send_angle[4]={0xEE,0x01,0x02,0x03};
	
	while(1)
	{
		if(time1_cntr>=200)  //0.2s
		{
			time1_cntr=0;
			LED_blink;
			send_angle[2]=(int16_t)(angle_x/32768*180+90);
			send_angle[3]=(int16_t)(angle_y/32768*180+90);
			for(int t=0;t<4;t++)
			{
				USART1->DR=send_angle[t];
				while((USART1->SR&0X40)==0);//等待发送结束
			}
		}
		move0(v1);
		move(v2);
		loopFOC0();
		loopFOC();
	}
}
void USART1_IRQHandler(void)   //串口1中断程序
{
	unsigned char Res;
	
	if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)  //
	{
		Res =USART_ReceiveData(USART1);	//读取接收到的字节
		
		if(USART_RX_STA==0)
		{
			if(Res==0xEE)
			{
				USART_RX_BUF[USART_RX_STA]=Res;
				USART_RX_STA=1;
			}
		}
		else if(USART_RX_STA==1)
		{
			if(Res==0x02)
			{
				USART_RX_BUF[USART_RX_STA]=Res;
				USART_RX_STA=2;
			}
			else
			{
				USART_RX_STA=0;
			}
		}
		else
		{
			USART_RX_BUF[USART_RX_STA]=Res;
			USART_RX_STA++;
			if(USART_RX_STA==8)
			{
				float set_angle_x=((float)USART_RX_BUF[2]-90)*32768/180;
				float set_angle_y=((float)USART_RX_BUF[3]-90)*32768/180;
				pid_setSetpoint(&pid_1,set_angle_x);
				pid_setSetpoint(&pid_2,set_angle_y);
				USART_RX_STA=0;
			}
		}
  }
}
void UART4_IRQHandler(void)   //串口4中断程序
{
	unsigned char Res;
	
	if(USART_GetITStatus(UART4, USART_IT_RXNE) != RESET)  //
	{
		Res =USART_ReceiveData(UART4);	//读取接收到的字节
		
		if(USART4_RX_STA==0)
		{
			if(Res==0x55)
			{
				USART4_RX_BUF[USART4_RX_STA]=Res;
				USART4_RX_STA=1;
			}
		}
		else if(USART4_RX_STA==1)
		{
			if(Res==0x53)
			{
				USART4_RX_BUF[USART4_RX_STA]=Res;
				USART4_RX_STA=2;
			}
			else
			{
				USART4_RX_STA=0;
			}
		}
		else
		{
			USART4_RX_BUF[USART4_RX_STA]=Res;
			USART4_RX_STA++;
			if(USART4_RX_STA==11)
			{
				USART4_RX_STA=0;
				angle_x=(float)((int16_t)(USART4_RX_BUF[2]+USART4_RX_BUF[3]*256));
				angle_y=(float)((int16_t)(USART4_RX_BUF[4]+USART4_RX_BUF[5]*256));
				v1 = -pid_update(&pid_1, angle_x);
				v2 = pid_update(&pid_2, angle_y);
			}
		}
  }
}



